#include <iostream>
#include "ros/ros.h"
#include "ros_mec_msg/ros_york_msg.h"
#include <string>
#include <string.h>
#include "ros_mec_msg/york.h"
#include "ros_mec_msg/motorv.h"

#define RX_BUFF_LEN 42
#define INFO 0

uint8_t Receive_Data_rx[RX_BUFF_LEN];
using namespace std;

int main(int argc, char **argv)
{
   // 解决中文乱码问题
   setlocale(LC_ALL, "");
   // 初始化一个节点
   ros::init(argc, argv, "ros_york");
   // 构建类管理对象
   TG_york_car TG_york;
   //ros::Rate rate(100); // 调整此处可以改变发布的速率
   // 上位机的主控制函数,while控制写在Hq_auto_control（）函数里面
   TG_york.TG_auto_control();
   ros::spin();
}

/**************************************
功能: 构造函数, 只执行一次，用于初始化
***************************************/
TG_york_car::TG_york_car()
{
   // 清空原有的共用体里面的数据,一定要清楚数据，不然会产生数据爆炸
   // memset(&rx_Imu_Mes.data, 0, sizeof(rx_Imu_Mes.data));     // 清除陀螺仪的6轴数据
   memset(&TG_Rece_car.data, 0,  sizeof(TG_Rece_car.data)); // 清除里程计的数据
   memset(&TG_Send_data.data, 0, sizeof(TG_Send_data.data)); // 清除ros下发的数据
   memset(&Send_MotorV.data,0,   sizeof(Send_MotorV.data));  //清除四个电机的数据
   ros::NodeHandle private_nh("TG_Car"); // 定义句柄 ，一个ros的节点可以有多个句柄来进行接口数据的访问,需要传入操纵的那个节点的名称
   /*
      获取参数服务器里面的数据，然后赋值给类结构体里面的成员，启用类参数服务器里面的数据是为了让多文件的launch文件进行调用
   */

   bool num1 = private_nh.param<string>("TG_usart_port_name", TG_usart_port_name, "/dev/ttyACM0");
   bool num2 = private_nh.param<int>("TG_serial_baud_rate", TG_serial_baud_rate, 115200);                                                     // 定义串口的波特率
   
   Cmd_vel_sub = Nh_TG.subscribe("/cmd_vel", 10, &TG_york_car::Cmdvel_callback, this);
   york_msg_pub = Nh_TG.advertise<ros_mec_msg::york>("mec_car",1);
   motor_vel_sub = Nh_TG.subscribe("/motor_vel",10,&TG_york_car::Motorvel_callback,this);
   // 在构造函数中进行开串口的操作
   ROS_INFO_STREAM("now! will open serial port!");
   try
   {
      // Attempts to initialize and open the serial port //尝试初始化与开启串口
      Usart_stm32serial.setPort(TG_usart_port_name);                // Select the serial port number to enable //选择要开启的串口号
      Usart_stm32serial.setBaudrate(TG_serial_baud_rate);           // Set the baud rate //设置波特率
      serial::Timeout _time = serial::Timeout::simpleTimeout(2000); // Timeout //超时等待
      Usart_stm32serial.setTimeout(_time);
      Usart_stm32serial.open(); // Open the serial port //开启串口
   }
   // serial::IOException 是 serial 库中定义的一个异常类，用于表示与串口通信相关的输入输出错误。&e 是一个引用，用于将异常对象存储在变量 e 中
   catch (serial::IOException &e)
   {
      ROS_ERROR_STREAM("tg_york can not open serial port,Please check the serial port cable! "); // 如果开启串口失败，打印错误信息
   }
   // 如果开启成功返回true
   if (Usart_stm32serial.isOpen())
   {
      ROS_INFO_STREAM("tg_york serial port opened"); // 串口开启成功提示
   }
}
/**************************************
功能: 循环获取下位机数据与发布话题
***************************************/
void TG_york_car::TG_auto_control()
{
   ros::Rate rate(50); // 调整此处可以改变发布的速率
   
   while (ros::ok())
   {
      bool flag = TG_Getserial_Data();
      //如果数据都是正确的，那么将数据发布出去
      if (flag == true)
      {
         bool flag_v  = TG_Mecmsg_Pub();
         if (flag_v == false)
         {
            // ROS_ERROR_STREAM("Chassis malfunction! stop!");
         }
         rate.sleep();
      }
      ros::spinOnce(); // The loop waits for the callback function //循环等待回调函数
   }
}
/**************************************
功能: 析构函数, 只执行一次，清理内存和最后一次发送数据
***************************************/
TG_york_car::~TG_york_car()
{
   ROS_INFO("Thank you for using our company's products!");
}

/**************************************
功能: 串口通讯校验函数，数据包n有个字节，第n-1个字节为校验位，第n个字节位帧尾。第1个字节到第n-2个字节数据按位异或的结果与第n-1个字节对比，即为BCC校验
输入参数： Count_Number：数据包前几个字节加入校验   mode：对发送数据还是接收数据进行校验,buf：传入的需要验证的数组地址
***************************************/
unsigned char TG_york_car::Check_Sum(unsigned char Count_Number, unsigned char mode)
{
   unsigned char check_sum = 0, k;

   if (mode == 0) // Receive data mode //接收数据模式
   {
      for (k = 0; k < Count_Number; k++)
      {
         check_sum = check_sum^Receive_Data_rx[k]; // By bit or by bit //按位异或
      }
   }

   if (mode == 1) // Send data mode //发送数据模式
   {
      for (k = 0; k < Count_Number; k++)
      {
         check_sum = check_sum^TG_Send_data.data[k]; // By bit or by bit //按位异或
      }
   }

   if (mode == 2) // Send data mode //发送数据模式
   {
      for (k = 0; k < Count_Number; k++)
      {
         check_sum = check_sum^Send_MotorV.data[k]; // By bit or by bit //按位异或
      }
   }
   return check_sum; // Returns the bitwise XOR result //返回按位异或结果
}
/**************************************
功能: 得到下位机串口发送过来的数据imu，odom等数据
***************************************/
bool TG_york_car::TG_Getserial_Data(void) // 定义接受串口数据的函数
{
   uint8_t check;
   size_t n = Usart_stm32serial.available(); // 判断是否接收到了字节数据
   //ROS_INFO("接收的数据长度为%ld", n);
   if (n != 0)
   {
      n = Usart_stm32serial.read(Receive_Data_rx, n); // 读取字节长度，看是不是需求的
      if (n != 42)                                    // 如果字节长度超过27，就将数据长度定为27
      {
         n = Usart_stm32serial.read(Receive_Data_rx, 42);
      }
      //ROS_INFO("接收的数据长度为%ld", n);

      if ((Receive_Data_rx[0] == Header) && (Receive_Data_rx[1] == Headerframe)) // 帧头读取正确
      {
        // ROS_INFO("接收到帧头Header：%x, %x", Header, Headerframe);
      }
      else
      {
        // ROS_INFO("并未接收到帧头,等待接收成功ing");
         // ROS_ERROR_STREAM("接收错误");
         memset(Receive_Data_rx, 0, sizeof(Receive_Data_rx));
      }
      if (Receive_Data_rx[36] == Ender && Receive_Data_rx[37] == Enderframe) // 帧尾读取正确
      {
         //ROS_INFO("接收到帧尾Ender：%x, %x", Ender, Enderframe);
         // if (Receive_Data_rx[41] == Check_Sum(41, 0)) // 如果接收到的校验位置和上位机计算的校验为相同，则表示数据传输的无误
         // {
            for(int j = 0 ;j<RX_BUFF_LEN;j++)
            {
               TG_Rece_car.data[j] = Receive_Data_rx[j];
            }
            #if INFO == 1
            ROS_INFO("Motor_Va = %f", TG_Rece_car.Car_Data.Motor_Va);
            ROS_INFO("Motor_Vb = %f", TG_Rece_car.Car_Data.Motor_Vb);
            ROS_INFO("Motor_Vc = %f", TG_Rece_car.Car_Data.Motor_Vc);
            ROS_INFO("Motor_Vd = %f", TG_Rece_car.Car_Data.Motor_Vd);
            ROS_INFO("Vx = %f", TG_Rece_car.Car_Data.Vx);
            ROS_INFO("Vy = %f", TG_Rece_car.Car_Data.Vy);
            ROS_INFO("Vz = %f", TG_Rece_car.Car_Data.Vz);
            #endif
            memset(Receive_Data_rx, 0, sizeof(Receive_Data_rx));
            return true;
         // }
      }
      else
      {
         return false;
      }
   }
}

/*将得到的数据发布出去 */
bool TG_york_car::TG_Mecmsg_Pub(void)
{
   ros_mec_msg::york Tg_york;
   Tg_york.Motor_Va = TG_Rece_car.Car_Data.Motor_Va;
   Tg_york.Motor_Vb = TG_Rece_car.Car_Data.Motor_Vb;
   Tg_york.Motor_Vc = TG_Rece_car.Car_Data.Motor_Vc;
   Tg_york.Motor_Vd = TG_Rece_car.Car_Data.Motor_Vd;
   Tg_york.Vx = TG_Rece_car.Car_Data.Vx;
   Tg_york.Vy = TG_Rece_car.Car_Data.Vy;
   Tg_york.Vz = TG_Rece_car.Car_Data.Vz;
   Tg_york.Power_Battery = TG_Rece_car.Car_Data.Power_Battery;
   Tg_york.error = TG_Rece_car.Car_Data.error;
   york_msg_pub.publish(Tg_york);
   if(Tg_york.error != 0x01 ) //说明下位机底盘已经发生故障了
   {
      return false;
   }
   else
   {
      return true;
   }

}
/*订阅ros的cmd_vel话题，然后发布给stm32进行通讯*/
void TG_york_car::Cmdvel_callback(const geometry_msgs::Twist::ConstPtr &TGcmd_vel)
{
      // 填充数据
   TG_Send_data.robot_send_data.HEADER = 0XEA;
   TG_Send_data.robot_send_data.HEADERFRAME = 0XAE;
   TG_Send_data.robot_send_data.x_speed = TGcmd_vel->linear.x*1000.0;  // 发送订阅的X线速度
   TG_Send_data.robot_send_data.y_speed = TGcmd_vel->linear.y*1000.0;  // 发送订阅的Y线速度
   TG_Send_data.robot_send_data.z_speed = TGcmd_vel->angular.z*1000.0; // 发送订阅的z轴角速度
   ///TG_Send_data.robot_send_data.car_mode = zskj_iodate;     //io控制模式 ，1为曼克纳姆轮，2为四驱，3为阿克曼，4为差速
  // TG_Send_data.robot_send_data.car_flagconfirm = 1; //
   TG_Send_data.robot_send_data.stop_flag = 0X02;
   TG_Send_data.robot_send_data.ENDER = 0XFD;
   TG_Send_data.robot_send_data.ENDERFRAME = 0XDF;
   TG_Send_data.robot_send_data.checknum = 0x03;
   //ROS_INFO("checknum = %d", TG_Send_data.robot_send_data.checknum);
   try
   {
      Usart_stm32serial.write(TG_Send_data.data, sizeof(TG_Send_data.data)); // Sends data to the downloader via serial port //通过串口向下位机发送数据
      // ROS_INFO("写入成功！");
   }
   catch (serial::IOException &e)
   {
      ROS_ERROR_STREAM("Unable to send data through serial port"); // If sending data fails, an error message is printed //如果发送数据失败，打印错误信息
   }
}

/*用户订阅这个话题之后就可以得到控制轮子的速度 */
void TG_york_car::Motorvel_callback(const ros_mec_msg::motorv::ConstPtr &TGmotor_vel)
{
   Send_MotorV.TG_Motor.HeaderA = 0xBE;
   Send_MotorV.TG_Motor.HeaderAframe =0XEB; 
   Send_MotorV.TG_Motor.start_flag = 0x03;
   Send_MotorV.TG_Motor.MotorA_v = TGmotor_vel->MotorA_v; //订阅/motor_vel，并且进行速度的赋值操作
   Send_MotorV.TG_Motor.MotorB_v = TGmotor_vel->MotorB_v;
   Send_MotorV.TG_Motor.MotorC_v = TGmotor_vel->MotorC_v;
   Send_MotorV.TG_Motor.MotorD_v = TGmotor_vel->MotorD_v;
   Send_MotorV.TG_Motor.stop_flag =0X02;   // 停止位
   Send_MotorV.TG_Motor.ENDER =0XED;       // 数据帧尾
   Send_MotorV.TG_Motor.ENDERFRAME =0XDE; // 数据帧尾2
   Send_MotorV.TG_Motor.checknum = 0x03;   // 校验位
   try
   {
      Usart_stm32serial.write(Send_MotorV.data, sizeof(Send_MotorV.data)); // Sends data to the downloader via serial port //通过串口向下位机发送数据
      // ROS_INFO("写入成功！");
   }
   catch (serial::IOException &e)
   {
      ROS_ERROR_STREAM("Unable to send data through serial port"); // If sending data fails, an error message is printed //如果发送数据失败，打印错误信息
   }
}
